
#pragma once
#include "Matrix4.h"

Matrix4::Matrix4(int dim )
{
	_dim = dim;
	_size = dim * dim;
	identity();
}
Matrix4::Matrix4(double a1, double a2, double a3,
	double b1, double b2, double b3,
	double c1, double c2, double c3)
{
	set(a1, a2, a3, b1, b2, b3, c1, c2, c3);
	_dim = 4;
	_size = 16;
}
Matrix4::Matrix4(double a1, double a2, double a3, double a4,
	double b1, double b2, double b3, double b4,
	double c1, double c2, double c3, double c4,
	double d1, double d2, double d3, double d4)
{
	set(a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4);
	_dim = 4;
	_size = 16;
}
Matrix4::Matrix4(Vector3D u, Vector3D v, Vector3D n)
{
	set(u, v, n);
}

Vector3D operator *(Vector3D& T, Matrix4& M)
{
	Vector3D result;

	result[0] =
		T[0] * M._data[0] +
		T[1] * M._data[4] +
		T[2] * M._data[8] +
		T[3] * M._data[12];

	result[1] =
		T[0] * M._data[1] +
		T[1] * M._data[5] +
		T[2] * M._data[9] +
		T[3] * M._data[13];

	result[2] =
		T[0] * M._data[2] +
		T[1] * M._data[6] +
		T[2] * M._data[10] +
		T[3] * M._data[14];

	result[3] =
		T[0] * M._data[3] +
		T[1] * M._data[7] +
		T[2] * M._data[11] +
		T[3] * M._data[15];
	return result;
}
 Vector3D operator *(const Matrix4& M, const Vector3D& T)
{
	Vector3D result;
	result[0] =
		T[0] * M._data[0] +
		T[1] * M._data[1] +
		T[2] * M._data[2] +
		T[3] * M._data[3];

	result[1] =
		T[0] * M._data[4] +
		T[1] * M._data[5] +
		T[2] * M._data[6] +
		T[3] * M._data[7];

	result[2] =
		T[0] * M._data[8] +
		T[1] * M._data[9] +
		T[2] * M._data[10] +
		T[3] * M._data[11];

	result[3] =
		T[0] * M._data[12] +
		T[1] * M._data[13] +
		T[2] * M._data[14] +
		T[3] * M._data[15];
	return result;
}
 Matrix4 operator *(const Matrix4& M1, const Matrix4& M2)
{
	Matrix4 result;
	result._data[0] = M1._data[0] * M2._data[0] +
		M1._data[1] * M2._data[4] +
		M1._data[2] * M2._data[8] +
		M1._data[3] * M2._data[12];

	result._data[1] = M1._data[0] * M2._data[1] +
		M1._data[1] * M2._data[5] +
		M1._data[2] * M2._data[9] +
		M1._data[3] * M2._data[13];

	result._data[2] = M1._data[0] * M2._data[2] +
		M1._data[1] * M2._data[6] +
		M1._data[2] * M2._data[10] +
		M1._data[3] * M2._data[14];

	result._data[3] = M1._data[0] * M2._data[3] +
		M1._data[1] * M2._data[7] +
		M1._data[2] * M2._data[11] +
		M1._data[3] * M2._data[15];

	//
	// row 2
	//
	result._data[4] = M1._data[4] * M2._data[0] +
		M1._data[5] * M2._data[4] +
		M1._data[6] * M2._data[8] +
		M1._data[7] * M2._data[12];

	result._data[5] = M1._data[4] * M2._data[1] +
		M1._data[5] * M2._data[5] +
		M1._data[6] * M2._data[9] +
		M1._data[7] * M2._data[13];

	result._data[6] = M1._data[4] * M2._data[2] +
		M1._data[5] * M2._data[6] +
		M1._data[6] * M2._data[10] +
		M1._data[7] * M2._data[14];

	result._data[7] = M1._data[4] * M2._data[3] +
		M1._data[5] * M2._data[7] +
		M1._data[6] * M2._data[11] +
		M1._data[7] * M2._data[15];

	//
	// row 3
	//
	result._data[8] = M1._data[8] * M2._data[0] +
		M1._data[9] * M2._data[4] +
		M1._data[10] * M2._data[8] +
		M1._data[11] * M2._data[12];

	result._data[9] = M1._data[8] * M2._data[1] +
		M1._data[9] * M2._data[5] +
		M1._data[10] * M2._data[9] +
		M1._data[11] * M2._data[13];

	result._data[10] = M1._data[8] * M2._data[2] +
		M1._data[9] * M2._data[6] +
		M1._data[10] * M2._data[10] +
		M1._data[11] * M2._data[14];

	result._data[11] = M1._data[8] * M2._data[3] +
		M1._data[9] * M2._data[7] +
		M1._data[10] * M2._data[11] +
		M1._data[11] * M2._data[15];

	//
	// row 4
	//
	result._data[12] = M1._data[12] * M2._data[0] +
		M1._data[13] * M2._data[4] +
		M1._data[14] * M2._data[8] +
		M1._data[15] * M2._data[12];

	result._data[13] = M1._data[12] * M2._data[1] +
		M1._data[13] * M2._data[5] +
		M1._data[14] * M2._data[9] +
		M1._data[15] * M2._data[13];

	result._data[14] = M1._data[12] * M2._data[2] +
		M1._data[13] * M2._data[6] +
		M1._data[14] * M2._data[10] +
		M1._data[15] * M2._data[14];

	result._data[15] = M1._data[12] * M2._data[3] +
		M1._data[13] * M2._data[7] +
		M1._data[14] * M2._data[11] +
		M1._data[15] * M2._data[15];

	return result;
}
 Matrix4 operator *(Matrix4& M, double& f)
{
	Matrix4 result;

	for(int i=0; i < 16; i++)
		result._data[i] = M._data[i] * f;

	return result;
}
 Matrix4 operator +(Matrix4& M1, Matrix4& M2)
{
	Matrix4 result;

	for(int i=0; i < 16; i++)
		result._data[i] = M1._data[i] + M2._data[i];

	return result;
}
 Matrix4 operator -(Matrix4& M1, Matrix4& M2)
{
	Matrix4 result;

	for(int i=0; i < 16; i++)
		result._data[i] = M1._data[i] - M2._data[i];

	return result;
}

void Matrix4::transpose()
{
	Matrix4 M;

	// note: _data 0, 5, 10 and 15 remain the same (diagonal)

	M._data[0] = _data[0];
	M._data[5] = _data[5];
	M._data[10] = _data[10];
	M._data[15] = _data[15];

	M._data[1] = _data[4];
	M._data[2] = _data[8];
	M._data[3] = _data[12];
	M._data[4] = _data[1];
	M._data[6] = _data[9];
	M._data[7] = _data[13];
	M._data[8] = _data[2];
	M._data[9] = _data[6];
	M._data[11] = _data[14];
	M._data[12] = _data[3];
	M._data[13] = _data[7];
	M._data[14] = _data[11];

	*this = M;
}

void Matrix4::set(double a1, double a2, double a3,
	double b1, double b2, double b3,
	double c1, double c2, double c3)
{
	set(a1, a2, a3, 0.0,
		b1, b2, b3, 0.0,
		c1, c2, c3, 0.0,
		0.0, 0.0, 0.0, 1.0);
}
void Matrix4::set(Vector3D u, Vector3D v, Vector3D n)
{
	set(u[Common::X], v[Common::X], n[Common::X], u[Common::Y], v[Common::Y], n[Common::Y], u[Common::Z], v[Common::Z], n[Common::Z]);
}
void Matrix4::set(double a1, double a2, double a3, double a4,
	double b1, double b2, double b3, double b4,
	double c1, double c2, double c3, double c4,
	double d1, double d2, double d3, double d4)
{
	_data[0] = a1;
	_data[1] = a2;
	_data[2] = a3;
	_data[3] = a4;

	_data[4] = b1;
	_data[5] = b2;
	_data[6] = b3;
	_data[7] = b4;

	_data[8] = c1;
	_data[9] = c2;
	_data[10] = c3;
	_data[11] = c4;

	_data[12] = d1;
	_data[13] = d2;
	_data[14] = d3;
	_data[15] = d4;
}
double Matrix4::get(int index1, int index2)
{
	return _data[index1 * _dim + index2];
}

void Matrix4::identity()
{
	clear();
	_data[0] = _data[5] = _data[10] = _data[15] = 1.0;
}
void Matrix4::clear()
{
	for(int i=0; i < 16; i++) _data[i] = 0;
}

void Matrix4::translate(double tx, double ty, double tz)
{
	identity();
	_data[3] = tx;
	_data[7] = ty;
	_data[11] = tz;
}
void Matrix4::translate(Vector3D T)
{
	translate(T[Common::X], T[Common::Y], T[Common::Z]);
}
void Matrix4::rotate(double ax, double ay, double az)
{
	Matrix4 RX(1.0, 0.0, 0.0,
		0.0, cos(ax), -sin(ax),
		0.0, sin(ax), cos(ax));

	Matrix4 RY(cos(ay), 0.0, -sin(ay),
		0.0, 1.0, 0.0,
		sin(ay), 0.0, cos(ay));

	Matrix4 RZ(cos(az), -sin(az), 0.0,
		sin(az), cos(az), 0.0,
		0.0, 0.0, 1.0);

	*this = RX * RY * RZ;
}
void Matrix4::rotate(Vector3D axis, double angle)
{
	double x = axis[Common::X];
	double y = axis[Common::Y];
	double z = axis[Common::Z];

	double c = cos(angle);
	double s = sin(angle);
	double t = 1 - c;

	set(t * x * x + c,     t * x * y + s * z,  t * x * z - s * y,
		t * x * y - s * z, t * y * y + c,      t * y * z + s * x,
		t * x * z + s * y, t * y * z - s * x,  t * z * z + c);
}
void Matrix4::scale(double sx, double sy, double sz)
{
	set(sx, 0, 0,
		0, sy, 0,
		0, 0, sz);
}
void Matrix4::scale(Vector3D S)
{
	scale(S[Common::X], S[Common::Y], S[Common::Z]);
}
void Matrix4::mirror(int xy, int xz, int yz)
{
	identity();

	if (xy) _data[10] = -1;
	if (xz) _data[5] = -1;
	if (yz) _data[0] = -1;
}


Matrix4* Matrix4::inverseMatrix()
{
	double det=this->det();
	if(det!=0)
	{

		Matrix4* adjointMatrix=this->adjointMatrix();
		Matrix4* pInverseProjectMatrix=new Matrix4;
		pInverseProjectMatrix->set((*adjointMatrix)[0]/det,(*adjointMatrix)[1]/det,(*adjointMatrix)[2]/det,//3
								   (*adjointMatrix)[4]/det,(*adjointMatrix)[5]/det,(*adjointMatrix)[6]/det,//7
								   (*adjointMatrix)[8]/det,(*adjointMatrix)[9]/det,(*adjointMatrix)[10]/det//,11	
									//12				//13				//14					//15
			);	
		return pInverseProjectMatrix;
	}	
	return NULL;
}

double Matrix4::det()
{
	double det=this->get(0,0)*(this->get(1,1)*this->get(2,2)-this->get(1,2)*this->get(2,1))
				-this->get(0,1)*(this->get(1,0)*this->get(2,2)-this->get(1,2)*this->get(2,0))
				+this->get(0,2)*(this->get(1,0)*this->get(2,1)-this->get(1,1)*this->get(2,0));
	return det;
}
Matrix4* Matrix4::adjointMatrix()
{
	Matrix4* AdjointMatrix=new Matrix4;

	double A00=this->get(1,1)*this->get(2,2)-this->get(1,2)*this->get(2,1);
	double A01=-(this->get(1,0)*this->get(2,2)-this->get(1,2)*this->get(2,0));
	double A02=this->get(1,0)*this->get(2,1)-this->get(1,1)*this->get(2,0);		
	double A10=-(this->get(0,1)*this->get(2,2)-this->get(0,2)*this->get(2,1));
	double A11=this->get(0,0)*this->get(2,2)-this->get(0,2)*this->get(2,0);
	double A12=-(this->get(0,0)*this->get(2,1)-this->get(0,1)*this->get(2,0));	
	double A20=this->get(0,1)*this->get(1,2)-this->get(0,2)*this->get(1,1);
	double A21=-(this->get(0,0)*this->get(1,2)-this->get(0,2)*this->get(1,0));
	double A22=this->get(0,0)*this->get(1,1)-this->get(0,1)*this->get(1,0);
	AdjointMatrix->set(A00,A01,A02,
		A10,A11,A12,
		A20,A21,A22						
		);	
	return AdjointMatrix;
}
